#include <avr/interrupt.h>
#include <avr/io.h> 
const int turn_radius =  155;
enum move_direction {w,a,s,d,wa,wd,sa,sd,f};

volatile move_direction move;
volatile boolean time_to_act;

//they'll be PWM
volatile int pin_left_enable = 5; 
volatile int pin_right_enable = 8;


//they'll be digital
volatile int pin_left_A = 3;
volatile int pin_left_B = 4;

volatile int pin_right_A = 9;
volatile int pin_right_B = 10;


//TO DO, imlement the PWM in all of this shit

void left_stop(){
    digitalWrite(pin_left_enable, LOW);
  digitalWrite(pin_left_A,LOW);
  digitalWrite(pin_left_B,LOW);
}

void right_stop(){
     digitalWrite(pin_right_enable, LOW);
  digitalWrite(pin_right_A,LOW);
  digitalWrite(pin_right_B,LOW); 
}

void right_forward(){
  digitalWrite(pin_left_enable, HIGH);
  digitalWrite(pin_left_A,HIGH);
  digitalWrite(pin_left_B,LOW); 
}

void right_reverse(){
  digitalWrite(pin_left_enable, HIGH);
  digitalWrite(pin_left_A,LOW);
  digitalWrite(pin_left_B,HIGH); 
}

void left_forward(){
  digitalWrite(pin_right_enable, HIGH);
  digitalWrite(pin_right_A,HIGH);
  digitalWrite(pin_right_B,LOW); 
}

void left_reverse(){
  digitalWrite(pin_right_enable, HIGH);
  digitalWrite(pin_right_A,LOW);
  digitalWrite(pin_right_B,HIGH); 
}

void right_partial_reverse(){
  analogWrite(pin_right_enable,turn_radius);
  digitalWrite(pin_right_A,LOW);
  digitalWrite(pin_right_B,HIGH); 
}

void right_partial_forward(){
  analogWrite(pin_right_enable,turn_radius);
  digitalWrite(pin_right_A,HIGH);
  digitalWrite(pin_right_B,LOW); 
}

void left_partial_reverse(){
  analogWrite(pin_left_enable,turn_radius);
  digitalWrite(pin_left_A,LOW);
  digitalWrite(pin_left_B,HIGH); 
}

void left_partial_forward(){
  analogWrite(pin_left_enable,turn_radius);
  digitalWrite(pin_left_A,HIGH);
  digitalWrite(pin_left_B,LOW); 
}



void full_steam_ahead(){
  left_forward();
  right_forward();
  
}

void port_side(){
  left_reverse();
  right_forward();
}

void starboard_maggots(){
    left_forward();
  right_reverse();  
}

void full_stop(){
  left_stop();
  right_stop();
  
}

void reverse_all_hamsters(){
  left_reverse();
  right_reverse(); 
}

void forward_port(){
  right_forward();
  left_partial_forward();
}

void forward_starboard(){
  right_partial_forward();
  left_forward();
}

void reverse_port(){
  left_partial_reverse();
  right_reverse();
}

void reverse_starboard(){
  right_partial_reverse();
  left_reverse();
}



void setup() {
  
  
  move = f;
  time_to_act = true;
  cli();//stop interrupts
 Serial.begin(9600);
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  OCR1A = 624;// = (16*10^6) / (1*1024) - 1 (must be <65536)   7624 is about 0.5 seconds
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS10 and CS12 bits for 1024 prescaler
  TCCR1B |= (1 << CS12) | (1 << CS10);  
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);

  //set pins to output
  pinMode(pin_left_enable, OUTPUT); 
  pinMode(pin_right_enable, OUTPUT); 
  
  pinMode(pin_left_A, OUTPUT); 
  pinMode(pin_left_B, OUTPUT); 
  
  pinMode(pin_right_A, OUTPUT); 
  pinMode(pin_right_B, OUTPUT); 
  
  
  sei();//allow interrupts 
  
}

void loop() {
  
  if(time_to_act == true){
    switch(move){
     case w:
       Serial.println("Full steam ahead");
        full_steam_ahead();
       break;
     
     case a:
       Serial.println("Port side to the max");
      port_side();
       break;
    
     case s:
       Serial.println("Reverse all hamsters");
    reverse_all_hamsters();
       break;
     
     case d:
       Serial.println("Starboard, you maggots");
      starboard_maggots();
       break;
       
     case wa:
      Serial.println("Forward and port, scum!");
      forward_port();
      break;
      
     case wd:
      Serial.println("Forward and starboard, bucko");
      forward_starboard();
      break;
      
     case sa:
      Serial.println("Reverse and port, scurvy rats");
      reverse_port();
      break;
      
     case sd:
      Serial.println("Reverse and starboard, rats");
      reverse_starboard();
      break;
     
     case f:
       Serial.println("Full Stop, you fools");
        full_stop();
       break;
     
     case 'i':
       Serial.println("You don't know what you're pressing.");
        full_stop();
       break;
    }
    time_to_act = false;
  }
}
inline void clear_input_buffer(){
  while(Serial.available() > 0)
  {
   Serial.read(); 
  }
}

inline void read_command(){
   char chars_read[3];
   chars_read[0] = Serial.read();
   chars_read[1] = Serial.read();
   chars_read[2] = Serial.read();
   if(chars_read[2] != '\n'){
    Serial.println("Third char wasn't a newline, it was:" + chars_read[2]);
    clear_input_buffer();
    return; 
   }
   else{

     String command = String(chars_read[0]);
     command += chars_read[1];
     Serial.println(command);
     //wtf switche statements y u no work with strings
     if(command == "ww"){
       move = w;
     }
     else if(command == "aa"){
      move = a;
     }
     else if(command == "ss"){
      move = s;
     }
     else if(command == "dd"){
      move = d;
     }
     else if(command == "wa"){
      move = wa;
     }
     else if(command == "wd"){
     move = wd; 
     }
     else if(command == "sa"){
      move = sa;
     }
     else if(command == "sd"){
      move = sd;
     }
     else if(command == "ff"){
      move = f;
     }
     else{
       Serial.println("you gave me garbage! What is this:" + command);
     }
   }
}


ISR(TIMER1_COMPA_vect){  //polling because wtf Arduino, where are the serial interruptse
   noInterrupts();
   static int i = 0;

   if(Serial.available() > 2)
   {
       time_to_act = true;
       read_command();
   }  
   interrupts();
}


